//
// Created by daybeha on 23-11-27.
//
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"

using namespace std;

class ComputeGravity : public rclcpp::Node {
public:
    ComputeGravity() : Node("imu_to_odom_tf_converter") {

//        imu_topic_ = this->declare_parameter("imu_topic", "/imu/sensor_msgs/Imu");
        imu_topic_ = this->declare_parameter("imu_topic", "/imu/data");
        cout << "subscribe imu_topic: " << imu_topic_ << endl;

        // 订阅IMU数据
        imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
                imu_topic_, 10, std::bind(&ComputeGravity::imuCallback, this, std::placeholders::_1));
    }

private:
    /// 订阅IMU数据， 去均值，去除奇异值，计算测量重力
    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_msg) {
        cnt++;
        // 处理IMU数据，进行必要的计算
        Eigen::Vector3d curr_acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);
        Eigen::Vector3d curr_gyro(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
        Eigen::Quaterniond Qwb = Eigen::Quaterniond(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z);
//        cout << "curr_acc: " << curr_acc(0) << " " << curr_acc(1) << " " << curr_acc(2) << endl;
//        cout << "curr_gyro: " << curr_gyro(0) << " " << curr_gyro(1) << " " << curr_gyro(2) << endl;
//        cout << "Qwb: " << Qwb.w() << " " << Qwb.x() << " " << Qwb.y() << " " << Qwb.z() << endl;


        Eigen::Vector3d curr_gw = Qwb.normalized() * curr_acc;
        if (cnt < 101){
            gw_sum += curr_gw;
        } else if(cnt > 100 && (curr_gw - gw).norm() < 0.025){
            gw_sum += curr_gw;
        } else{
            cnt--;
            cerr << "gravity measurement error, leave imu alone!" << endl;
            cout << "curr_gw - gw: " << (curr_gw - gw).norm() << endl;
        }

        if(cnt%100==0){
            gw = gw_sum / cnt;
            cout << cnt << "/ gw: " << gw(0) << ", " << gw(1) << ", " << gw(2) << endl;
        }
    }

    void imuCallback_no_Q(const sensor_msgs::msg::Imu::SharedPtr imu_msg) {
        // 处理IMU数据，进行必要的计算

        //ROS2中的时间类型builtin_interfaces::msg::Time是需要把秒和纳秒组合起来才能表示当前时间的
        double curr_time = imu_msg->header.stamp.sec + imu_msg->header.stamp.nanosec * 1e-9;
        Eigen::Vector3d curr_acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);
        Eigen::Vector3d curr_gyro(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
        Eigen::Quaterniond Qwb = Eigen::Quaterniond(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z);
//        cout << "curr_acc: " << curr_acc(0) << " " << curr_acc(1) << " " << curr_acc(2) << endl;
//        cout << "curr_gyro: " << curr_gyro(0) << " " << curr_gyro(1) << " " << curr_gyro(2) << endl;
//        cout << "Qwb: " << Qwb.w() << " " << Qwb.x() << " " << Qwb.y() << " " << Qwb.z() << endl;

        Eigen::Vector3d dtheta_half, acc_w;
        if(pre_time!=0){
            cnt++;
            double dt = curr_time - pre_time;
            dtheta_half = (curr_gyro + pre_gyro) * dt /2.0;
            acc_w = Qwb * (curr_acc + pre_acc)/ 2.0 + gw;  // aw = Pwb * ( acc_body - acc_bias ) + gw
//            cout << "acc: " << acc_w(0) << " " << acc_w(1) << " " << acc_w(2) << endl;

            Qwb = Qwb * Eigen::Quaterniond(1, dtheta_half[0]/2,
                                           dtheta_half[1]/2, dtheta_half[2]/2).normalized();

            Eigen::Vector3d curr_gw = Qwb * curr_acc;
            //        gw_sum += curr_gw;
            if (cnt < 101){
                gw_sum += curr_gw;
            } else if(cnt > 100 && (curr_gw - gw).norm() < 0.02 && (curr_acc - pre_acc).norm() < 0.1 && (curr_gyro - pre_gyro).norm() < 0.01){
                gw_sum += curr_gw;
                cout << "curr_gw - gw: " << (curr_gw - gw).norm() << endl;
            } else{
                cnt--;
                cerr << "gravity measurement error, leave imu alone!" << endl;
                cout << "curr_gw - gw: " << (curr_gw - gw).norm() << endl;
                cout << "curr_acc - pre_acc: " << (curr_acc - pre_acc).norm() << endl;
                cout << "curr_gyro - pre_gyro: " << (curr_gyro - pre_gyro).norm() << endl;
            }

            if(cnt%100==0){
                gw = gw_sum / cnt;
                cout << cnt << "/ gw: " << gw(0) << ", " << gw(1) << ", " << gw(2) << endl;
            }
        }

        pre_time = curr_time;
        pre_acc = curr_acc;
        pre_gyro = curr_gyro;
    }


    unsigned int cnt=0;
    string imu_topic_;

    double pre_time;
    Eigen::Vector3d pre_acc;
    Eigen::Vector3d pre_gyro;

    Eigen::Vector3d gw;          //  重力加速度
    Eigen::Vector3d gw_sum;

    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;

};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ComputeGravity>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
